#include <localization/test/fake_lamp_detector.h>

FakeLampDetector::FakeLampDetector(const std::vector<player_point_2d> &mapped_lamps,
        PlayerCc::SimulationProxy *simulation_proxy,
        SyncedLampCentroids *lamp_centroids) :
        gaussian_distribution_(0, 0.25),
                gaussian_generator_(random_generator_, gaussian_distribution_),
                lamp_centroids_(lamp_centroids),
                mapped_lamps_(mapped_lamps), simulation_proxy_(simulation_proxy) {
}

void FakeLampDetector::operator()() {
  char robot_name[6] = "robot";

  while (true) {
    // Gets the robot's position in the simulation (no uncertainty)
    player_pose2d robot_pose;
    simulation_proxy_->GetPose2d(robot_name, robot_pose.px, robot_pose.py,
            robot_pose.pa);

    // Adds to a vector all the mapped lamps within a 6.0m radius from the robot
    std::vector<player_point_2d> observed_lamps;

    for (std::vector<player_point_2d>::const_iterator it =
            mapped_lamps_.begin(); it != mapped_lamps_.end(); ++it) {
      double dx = it->px - robot_pose.px;
      double dy = it->py - robot_pose.py;
      double distance = sqrt(dx * dx + dy * dy);

      if (distance > 6.0)
        continue;

      double error = gaussian_generator_();

      player_point_2d observed_lamp;
      observed_lamp.px = dx * sin(robot_pose.pa) - dy * cos(robot_pose.pa) + error;
      observed_lamp.py = dx * cos(robot_pose.pa) + dy * sin(robot_pose.pa) + error;
      observed_lamps.push_back(observed_lamp);
    }

    lamp_centroids_->set_lamp_centroids(observed_lamps);

    boost::this_thread::sleep(
            boost::posix_time::milliseconds(kSleepTimeInMilliseconds));
  }
}

